﻿
#include "bounding_capsule.h"
#include <game/physics/physics_math.h>
#include <graphics/imath.h>
#include <game/physics/contact.h>

using namespace DirectX;

namespace ifire::game {
BoundingCapsule::BoundingCapsule()
    : start(XMFLOAT3(0.0f, 0.0f, 0.0f))
    , end(XMFLOAT3(0.0f, 1.0f, 0.0f))
    , radius(0.5f) {}

BoundingCapsule::BoundingCapsule(float s, float e, float r)
    : start(XMFLOAT3(0.0f, s, 0.0f)), end(XMFLOAT3(0.0f, e, 0.0f)), radius(r) {}

void BoundingCapsule::Transform(IVec worldPos, IVec rotation, IVec scaling) {
  const float min_scale = 1e-6f;
  IVec safe_scale = XMVectorMax(scaling, XMVectorReplicate(min_scale));

  // 缩放计算（Y轴独立处理）
  float scaledStart = start.y * XMVectorGetY(safe_scale);
  float scaledEnd = end.y * XMVectorGetY(safe_scale);
  float scaledRadius =
      radius * (std::max)({XMVectorGetX(safe_scale), XMVectorGetZ(safe_scale)});

  // 旋转和位移
  IMat rotMat = XMMatrixRotationQuaternion(rotation);
  IVec worldStart =
      XMVector3Transform(XMVectorSet(0, scaledStart, 0, 0), rotMat) + worldPos;
  IVec worldEnd =
      XMVector3Transform(XMVectorSet(0, scaledEnd, 0, 0), rotMat) + worldPos;

  // 更新状态
  XMStoreFloat3(&this->start, worldStart);
  XMStoreFloat3(&this->end, worldEnd);
  this->radius = scaledRadius;
}

bool BoundingCapsule::Intersects(const BoundingSphere& sphere) const {
  // 构建世界空间线段（Y轴对齐）
  IVec s = XMLoadFloat3(&start);
  IVec e = XMLoadFloat3(&end);
  IVec p = XMLoadFloat3(&sphere.Center);

  // 计算线段最近点
  IVec seg = e - s;
  IVec segLength = XMVector3Length(seg);
  IVec segDir = (XMVectorGetX(segLength) > FLT_EPSILON)
                    ? seg / segLength
                    : XMVectorSet(0, 1, 0, 0); // 处理零长度线段

  IVec proj = XMVector3Dot(p - s, segDir);
  proj = XMVectorClamp(proj, XMVectorZero(), segLength);
  IVec closest = s + proj * segDir;

  // 距离检测
  IVec delta = p - closest;
  float distSq = XMVectorGetX(XMVector3LengthSq(delta));
  float totalRadius = radius + sphere.Radius;

  return distSq <= (totalRadius * totalRadius);
}

bool BoundingCapsule::Intersects(const BoundingOrientedBox& box) const {
  // 构建世界空间线段（Y轴对齐）
  IVec s = XMLoadFloat3(&start);
  IVec e = XMLoadFloat3(&end);

  // 转换到OBB局部空间
  IMat boxInvRot = XMMatrixRotationQuaternion(
      XMQuaternionInverse(XMLoadFloat4(&box.Orientation)));

  s = XMVector3Transform(s - XMLoadFloat3(&box.Center), boxInvRot);
  e = XMVector3Transform(e - XMLoadFloat3(&box.Center), boxInvRot);

  // 计算线段到AABB的最近点
  IVec boxMin = -XMLoadFloat3(&box.Extents);
  IVec boxMax = XMLoadFloat3(&box.Extents);
  IVec closest =
      physics_math::ClosestPointOnSegmentToAABB(s, e, boxMin, boxMax);

  // 距离检测
  IVec delta = closest - XMLoadFloat3(&box.Center);
  float distSq = XMVectorGetX(XMVector3LengthSq(delta));
  return distSq <= (radius * radius);
}

bool BoundingCapsule::Intersects(const BoundingCapsule& other) const {
  // 构建世界空间线段（Y轴对齐）
  IVec a1 = XMLoadFloat3(&start);
  IVec a2 = XMLoadFloat3(&end);
  IVec b1 = XMLoadFloat3(&other.start);
  IVec b2 = XMLoadFloat3(&other.end);

  // 计算线段最近点对
  IVec closestA, closestB;
  physics_math::ClosestPointBetweenSegments(a1, a2, b1, b2, closestA, closestB);

  // 距离检测
  IVec delta = closestB - closestA;
  float distSq = XMVectorGetX(XMVector3LengthSq(delta));
  float totalRadius = radius + other.radius;

  return distSq <= (totalRadius * totalRadius);
}


} // namespace ifire::game